// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcMultipleCurve3.h"

//----------------------------------------------------------------------------
MgcMultipleCurve3::MgcMultipleCurve3 (int iSegments, MgcReal* afTime)
    :
    MgcMultipleCurve<MgcVector3>(iSegments,afTime)
{
}
//----------------------------------------------------------------------------
MgcVector3 MgcMultipleCurve3::GetNormal (MgcReal fTime) const
{
    MgcVector3 kVelocity = GetFirstDerivative(fTime);
    MgcVector3 kAcceleration = GetSecondDerivative(fTime);
    MgcReal fVDotV = kVelocity.Dot(kVelocity);
    MgcReal fVDotA = kVelocity.Dot(kAcceleration);
    MgcVector3 kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
    kNormal.Unitize();
    return kNormal;
}
//----------------------------------------------------------------------------
MgcVector3 MgcMultipleCurve3::GetBinormal (MgcReal fTime) const
{
    MgcVector3 kVelocity = GetFirstDerivative(fTime);
    MgcVector3 kAcceleration = GetSecondDerivative(fTime);
    MgcReal fVDotV = kVelocity.Dot(kVelocity);
    MgcReal fVDotA = kVelocity.Dot(kAcceleration);
    MgcVector3 kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
    kNormal.Unitize();
    kVelocity.Unitize();
    MgcVector3 kBinormal = kVelocity.Cross(kNormal);
    return kBinormal;
}
//----------------------------------------------------------------------------
void MgcMultipleCurve3::GetFrame (MgcReal fTime, MgcVector3& kPosition,
    MgcVector3& kTangent, MgcVector3& kNormal, MgcVector3& kBinormal) const
{
    kPosition = GetPosition(fTime);
    MgcVector3 kVelocity = GetFirstDerivative(fTime);
    MgcVector3 kAcceleration = GetSecondDerivative(fTime);
    MgcReal fVDotV = kVelocity.Dot(kVelocity);
    MgcReal fVDotA = kVelocity.Dot(kAcceleration);
    kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
    kNormal.Unitize();
    kTangent = kVelocity;
    kTangent.Unitize();
    kBinormal = kTangent.Cross(kNormal);
}
//----------------------------------------------------------------------------
MgcReal MgcMultipleCurve3::GetCurvature (MgcReal fTime) const
{
    MgcVector3 kVelocity = GetFirstDerivative(fTime);
    MgcReal fSpeedSqr = kVelocity.SquaredLength();

    const MgcReal fTolerance = 1e-06;
    if ( fSpeedSqr >= fTolerance )
    {
        MgcVector3 kAcceleration = GetSecondDerivative(fTime);
        MgcVector3 kCross = kVelocity.Cross(kAcceleration);
        MgcReal fNumer = kCross.Length();
        MgcReal fDenom = MgcMath::Pow(fSpeedSqr,1.5);
        return fNumer/fDenom;
    }
    else
    {
        // curvature is indeterminate, just return 0
        return 0.0;
    }
}
//----------------------------------------------------------------------------
MgcReal MgcMultipleCurve3::GetTorsion (MgcReal fTime) const
{
    MgcVector3 kVelocity = GetFirstDerivative(fTime);
    MgcVector3 kAcceleration = GetSecondDerivative(fTime);
    MgcVector3 kCross = kVelocity.Cross(kAcceleration);
    MgcReal fDenom = kCross.SquaredLength();

    const MgcReal fTolerance = 1e-06;
    if ( fDenom >= fTolerance )
    {
        MgcVector3 kJerk = GetThirdDerivative(fTime);
        MgcReal fNumer = kCross.Dot(kJerk);
        return fNumer/fDenom;
    }
    else
    {
        // torsion is indeterminate, just return 0
        return 0.0;
    }
}
//----------------------------------------------------------------------------
